% 按照论文中的推导，使用matlab自带lsqnonlin的函数计算PDG 自由函数选用第一类切比雪夫多项式 函数原理，切比雪夫多项式
% 适用于在给定区间上的函数逼近，通过最小二乘法，将给定函数逼近为切比雪夫多项式的线性组合
% 使用切比雪夫多项式拟合自由函数g，使用第一类切比雪夫多项式
clear all;close all;clc;
%global PDG_TFC_constant
%PDG_TFC_constant = PDG_TFC_constant_C();
% 初始化猜测时间t0_guess t1 t2 tf_guess
%PDG_TFC_parmeters1 = PDG_TFC_constant(0,32.418,38.828,44.823);
% 初始化轨迹(线性插值)
% t = linspace(0, PDG_TFC_parmeters.tf_guess, PDG_TFC_constant.discretize_points);
% r =    zeros(size(PDG_TFC_constant.r0,1),PDG_TFC_constant.discretize_points);
% v = zeros(size(PDG_TFC_constant.v0,1),PDG_TFC_constant.discretize_points);
% a = zeros(size(PDG_TFC_constant.mar_g,1),PDG_TFC_constant.discretize_points);
% for i=1:1:size(PDG_TFC_constant.r0,1)
%     r(i,:) = linspace(PDG_TFC_constant.r0(i), PDG_TFC_constant.rf(i), PDG_TFC_constant.discretize_points);
% end
% for i=1:1:size(PDG_TFC_constant.v0,1)
%     v(i,:) = linspace(PDG_TFC_constant.v0(i), PDG_TFC_constant.vf(i), PDG_TFC_constant.discretize_points);
% end
% for i=1:1:size(PDG_TFC_constant.mar_g,1)
%     a(i,:) = PDG_TFC_constant.mar_g(i)*ones(1,PDG_TFC_constant.discretize_points);
% end
% 配点法最优分布 离散点长度三段都为 discretize_points 
options = optimoptions('fsolve', 'Display', 'iter', 'TolFun', 1e-6);
fun_cal = @(t_pam) PDG_solve(t_pam(1),t_pam(2),t_pam(3));
x = fsolve(fun_cal, [20;30;40], options);
function [residual]=PDG_solve(t1,t2,tf)
PDG_TFC_parmeters1 = PDG_TFC_constant(0,t1,t2,tf);
t = [chebyshev_points(PDG_TFC_constant.discretize_points,PDG_TFC_parmeters1.t0_guess,PDG_TFC_parmeters1.t1),...
    chebyshev_points(PDG_TFC_constant.discretize_points,PDG_TFC_parmeters1.t1,PDG_TFC_parmeters1.t2),...
    chebyshev_points(PDG_TFC_constant.discretize_points,PDG_TFC_parmeters1.t2,PDG_TFC_parmeters1.tf_guess)];
% 按照时间的插值点线性计算轨迹
disp(t1);disp(t2);disp(tf);
disp(t);
t_interp = t./PDG_TFC_parmeters1.tf_guess;
t_interp(end) = 1;
%disp(t_interp);
r_init = zeros(size(PDG_TFC_constant.r0,1),3*PDG_TFC_constant.discretize_points);
r_init(1,:) = interp1([0 1],[PDG_TFC_constant.r0(1) PDG_TFC_constant.rf(1)],t_interp);
r_init(2,:) = interp1([0 1],[PDG_TFC_constant.r0(2) PDG_TFC_constant.rf(2)],t_interp);
r_init(3,:) = interp1([0 1],[PDG_TFC_constant.r0(3) PDG_TFC_constant.rf(3)],t_interp);
v_init(1,:) = interp1([0 1],[PDG_TFC_constant.v0(1) PDG_TFC_constant.vf(1)],t_interp);
v_init(2,:) = interp1([0 1],[PDG_TFC_constant.v0(2) PDG_TFC_constant.vf(2)],t_interp);
v_init(3,:) = interp1([0 1],[PDG_TFC_constant.v0(3) PDG_TFC_constant.vf(3)],t_interp);
% lamda_v0 = v_init(:,1)/norm(v_init(1:3,1),2);
% lamda_vf = -r_init(:,1)/norm(r_init(1:3,1),2);
L_i = @(x) coeff_init_cal(x,t,r_init,v_init,PDG_TFC_parmeters1);
options_i = optimoptions('lsqnonlin', 'Display', 'iter','FunctionTolerance', 1e-10, 'StepTolerance', 1e-10);%,"Algorithm","levenberg-marquardt");
[x_i, resnorm_i, residual_i, exitflag_i, output_i] = lsqnonlin(L_i,zeros(9*PDG_TFC_constant.Chebyshev_N+18,1),[],[],options_i);
%[x_i, resnorm_i, residual_i, exitflag_i, output_i] = lsqnonlin(L_i,ones(9*PDG_TFC_constant.Chebyshev_N,1),[],[],options_i);
disp(exitflag_i);  % 1 表示正常收敛，3 表示容差触发停止
disp(output_i);    % 查看详细优化过程
%% 
% 生成初始猜测
% param_init = [x_i;
%             r_init(:,PDG_TFC_constant.discretize_points);v_init(:,PDG_TFC_constant.discretize_points);
%             r_init(:,2*PDG_TFC_constant.discretize_points);v_init(:,2*PDG_TFC_constant.discretize_points);
%             ones(6,1)];   
% dy_dx(1,:) = gradient(v_init(1,:), t);
% dy_dx(2,:) = gradient(v_init(2,:), t);
% dy_dx(3,:) = gradient(v_init(3,:), t);
%dd = mean(dy_dx,2)/norm(v_init(1:3,1),2);
% param_init = [x_i;lamda_x0(1);lamda_v0(1);lamda_x0(2);lamda_v0(2);lamda_x0(3);lamda_v0(3)];
%param_init = [x_i;ones(6,1)];
param_init = [x_i];
%[r_i,v_i]=calculate_trajectory(param_init(1:9*PDG_TFC_constant.Chebyshev_N+12,1),t,PDG_TFC_parmeters1);
%%%%%%%%%%% 绘图 %%%%%%%%%%%
% figure();
% plot(t,r_init(1,:),'r',t,r_init(2,:),'g',t,r_init(3,:),'b','LineWidth',1.5);hold on;
% legend('x','y','z')
% % xlabel('t/s')
% % ylabel('r/m')
% title("位置初始化直线轨迹")
% figure();
% plot(t,v_init(1,:),'r--',t,v_init(2,:),'g--',t,v_init(3,:),'b--','LineWidth',1.5);hold on;
% legend('vx','vy','vz')
% title("速度初始化直线轨迹")
% figure();
% plot(t,r_i(1,:),'r',t,r_i(2,:),'g',t,r_i(3,:),'b','LineWidth',1.5);hold on;
% legend('x','y','z')
% title("位置初始化切比雪夫轨迹")
% figure();
% plot(t,v_i(1,:),'r--',t,v_i(2,:),'g--',t,v_i(3,:),'b--','LineWidth',1.5);hold on;
% legend('vx','vy','vz')
% title("速度初始化切比雪夫轨迹")
%%%%%%%%%%%------%%%%%%%%%%%
% r = zeros(size(PDG_TFC_constant.r0,1),3*PDG_TFC_constant.discretize_points);
% v = zeros(size(PDG_TFC_constant.v0,1),3*PDG_TFC_constant.discretize_points);
% a = zeros(size(PDG_TFC_constant.mar_g,1),3*PDG_TFC_constant.discretize_points);
% for i=1:1:size(PDG_TFC_constant.r0,1)
%     r(i,:) = linspace(PDG_TFC_constant.r0(i), PDG_TFC_constant.rf(i), 3*PDG_TFC_constant.discretize_points);
% end
% for i=1:1:size(PDG_TFC_constant.v0,1)
%     v(i,:) = linspace(PDG_TFC_constant.v0(i), PDG_TFC_constant.vf(i), 3*PDG_TFC_constant.discretize_points);
% end
% for i=1:1:size(PDG_TFC_constant.mar_g,1)
%     a(i,:) = PDG_TFC_constant.mar_g(i)*ones(1,3*PDG_TFC_constant.discretize_points);
% end
%L_1_1 非线性最小二乘 损失函数9N+1 三段三维加速度向量9N 加哈密顿函数1 N为离散点数 变量顺序按此排列
% 待拟合参数 9*m m为切比雪夫多项式个数 r1--3 v1--3 r2--3 v2--3  lamdav协态根据PMP为0得到线性有两个参数 2*3 = 6 9m+18 变量顺序按此排列
% 使用切比雪夫多项式差值 先完成一段的拟合
%L_1_1 = @(x) PDG_L_1_1(, r, v, a, m0, tf_guess, ag,  , Tmin, Tmax, m, h, z);
L = @(x) PDG_3stage(x, t,PDG_TFC_parmeters1);%, r, v, a);
options = optimoptions('lsqnonlin', 'Display', 'iter', 'FunctionTolerance', 1e-10, 'StepTolerance', 1e-10);%,'Algorithm', 'levenberg-marquardt');
[x, resnorm, residual, exitflag, output] = lsqnonlin(L,param_init,[],[],options);
disp(exitflag);  % 1 表示正常收敛，3 表示容差触发停止
disp(output);    % 查看详细优化过程
% 需要注意单位换算的问题 根据求解的解x计算最优轨迹
% v1_r = zeros(3,PDG_TFC_constant.discretize_points);
% a1_r = zeros(3,PDG_TFC_constant.discretize_points);
% 根据优化结果计算轨迹
[r,v,a]=calculate_trajectory(x,t,PDG_TFC_parmeters1);
fprintf("t0_guess:%.4f,t1:%.4f,t2:%.4f,tf_guess:%.4f\n",PDG_TFC_parmeters1.t0_guess,PDG_TFC_parmeters1.t1,PDG_TFC_parmeters1.t2,PDG_TFC_parmeters1.tf_guess);
m_used = PDG_TFC_constant.alpha*...
    (PDG_TFC_constant.Tmax*(PDG_TFC_parmeters1.tf_guess-PDG_TFC_parmeters1.t2+PDG_TFC_parmeters1.t1-PDG_TFC_parmeters1.t0_guess)+...
    PDG_TFC_constant.Tmin*(PDG_TFC_parmeters1.t2-PDG_TFC_parmeters1.t1));
fprintf("m_used:%.4f\n",m_used);
% fprintf("m_used:%.4f\n",PDG_TFC_constant.alpha*...
%         (PDG_TFC_constant.Tmax*(PDG_TFC_parmeters1.tf_guess-PDG_TFC_parmeters1.t2+PDG_TFC_parmeters1.t1-PDG_TFC_parmeters1.t0_guess)+...
%         PDG_TFC_constant.Tmin*(PDG_TFC_parmeters1.t2-PDG_TFC_parmeters1.t1)));
% fsolve 最小化residual


%%%%%%%%%%% 绘图 %%%%%%%%%%%
% figure();
% plot(t,r(1,:),'r',t,r(2,:),'g',t,r(3,:),'b','LineWidth',1.5);hold on;
% legend('x','y','z')
% title("优化位置轨迹")
% figure();
% plot(t,v(1,:),'r--',t,v(2,:),'g--',t,v(3,:),'b--','LineWidth',1.5);hold on;
% legend('vx','vy','vz')
% title("优化速度轨迹")
% figure();
% plot(t,a(1,:),'r--',t,a(2,:),'g--',t,a(3,:),'b--','LineWidth',1.5);hold on;
% legend('ax','ay','az')
% title("优化加速度轨迹")
% figure();
% plot(t,(a(1,:).^2+a(2,:).^2+a(3,:).^2).^0.5,'r--','LineWidth',1.5);hold on;
% legend('a')
% title("优化总加速度轨迹")
%%%%%%%%%%%------%%%%%%%%%%%
% 求解协态lamda_m x中包含速度协态lamda_v
lamda_v_r(1,:) = x(9*PDG_TFC_constant.Chebyshev_N+13)+x(9*PDG_TFC_constant.Chebyshev_N+14)*t;
lamda_v_r(2,:) = x(9*PDG_TFC_constant.Chebyshev_N+15)+x(9*PDG_TFC_constant.Chebyshev_N+16)*t;
lamda_v_r(3,:) = x(9*PDG_TFC_constant.Chebyshev_N+17)+x(9*PDG_TFC_constant.Chebyshev_N+18)*t;
% 优化的推力解
%for i=1:1:size(lamda_v_r,2)
T(1,:) = -[lamda_v_r(1,1:PDG_TFC_constant.discretize_points).*PDG_TFC_constant.Tmax,...
        lamda_v_r(1,PDG_TFC_constant.discretize_points+1:2*PDG_TFC_constant.discretize_points).*PDG_TFC_constant.Tmin,...
       lamda_v_r(1,2*PDG_TFC_constant.discretize_points+1:3*PDG_TFC_constant.discretize_points).*PDG_TFC_constant.Tmax]./vecnorm(lamda_v_r);
T(2,:) = -[lamda_v_r(2,1:PDG_TFC_constant.discretize_points).*PDG_TFC_constant.Tmax,...
        lamda_v_r(2,PDG_TFC_constant.discretize_points+1:2*PDG_TFC_constant.discretize_points).*PDG_TFC_constant.Tmin,...
       lamda_v_r(2,2*PDG_TFC_constant.discretize_points+1:3*PDG_TFC_constant.discretize_points).*PDG_TFC_constant.Tmax]./vecnorm(lamda_v_r);
T(3,:) = -[lamda_v_r(3,1:PDG_TFC_constant.discretize_points).*PDG_TFC_constant.Tmax,...
        lamda_v_r(3,PDG_TFC_constant.discretize_points+1:2*PDG_TFC_constant.discretize_points).*PDG_TFC_constant.Tmin,...
        lamda_v_r(3,2*PDG_TFC_constant.discretize_points+1:3*PDG_TFC_constant.discretize_points).*PDG_TFC_constant.Tmax]./vecnorm(lamda_v_r);
%end
%a_t = T./(PDG_TFC_constant.m0*PDG_TFC_constant.g0);
a_t = PDG_TFC_constant.mar_g - a;
% 判断两者是否相等
T_norm = T./vecnorm(T);
a_t_norm = a_t./vecnorm(a_t);
vecnorm(T-a_t,2,1);
%%%%%%%%%%% 绘图 %%%%%%%%%%%
% figure();
% plot3(r(1,:),r(2,:),r(3,:),'r','LineWidth',1.5);hold on;
% quiver3(r(1,:),r(2,:),r(3,:),-a_t(1,:),-a_t(2,:),-a_t(3,:),'black','LineWidth',1.5);hold on;
% %axis equal;
% legend('轨迹')
% xlabel('x/m')
% ylabel('y/m')
% zlabel('z/m')
% title("优化轨迹和推力方向")
%%%%%%%%%%%------%%%%%%%%%%%

%%%%%%%%%%% 协态变量计算 %%%%%%%%%%%
% L_lamda_m = @(coeff_lamda_m) PDG_L_lamda_m(coeff_lamda_m,t,lamda_v_r,PDG_TFC_parmeters1);
% options_lamda_m = optimoptions('lsqnonlin', 'Display', 'iter','FunctionTolerance', 1e-10, 'StepTolerance', 1e-10);%,'Algorithm', 'levenberg-marquardt');
% [x_lamda_m, resnorm_lamda_m, residual_lamda_m, exitflag_lamda_m, output_lamda_m] = lsqnonlin(L_lamda_m,zeros(3*PDG_TFC_constant.discretize_points,1),[],[],options_lamda_m);

% lamda_m_r = calculate_lamda_m(x_lamda_m,t,PDG_TFC_parmeters1);
%  %% ode45验证算法
% % 去除重复值
% t_opt=[t(1,1:PDG_TFC_constant.discretize_points),...
%     t(1,PDG_TFC_constant.discretize_points+2:2*PDG_TFC_constant.discretize_points),...
%     t(1,2*PDG_TFC_constant.discretize_points+2:3*PDG_TFC_constant.discretize_points)];
% r_opt=[r(1:3,1:PDG_TFC_constant.discretize_points),...
%     r(1:3,PDG_TFC_constant.discretize_points+2:2*PDG_TFC_constant.discretize_points),...
%     r(1:3,2*PDG_TFC_constant.discretize_points+2:3*PDG_TFC_constant.discretize_points)];
% v_opt=[v(1:3,1:PDG_TFC_constant.discretize_points),...
%     v(1:3,PDG_TFC_constant.discretize_points+2:2*PDG_TFC_constant.discretize_points),...
%     v(1:3,2*PDG_TFC_constant.discretize_points+2:3*PDG_TFC_constant.discretize_points)];
% a_opt=[a(1:3,1:PDG_TFC_constant.discretize_points),...
%     a(1:3,PDG_TFC_constant.discretize_points+2:2*PDG_TFC_constant.discretize_points),...
%     a(1:3,2*PDG_TFC_constant.discretize_points+2:3*PDG_TFC_constant.discretize_points)];
% %使用ode45验证轨迹
% [t_ode,y_ode]=ode45(@(t_cal,state) dof3_ode(t_cal,state,t_opt,a_opt),[0 PDG_TFC_parmeters1.tf_guess],[PDG_TFC_constant.r0;PDG_TFC_constant.v0]);
% figure();
% plot(t_ode,y_ode(:,1),'r',t_ode,y_ode(:,2),'g',t_ode,y_ode(:,3),'b','LineWidth',1.5);hold on;
% legend('x','y','z')
% title("线性插值加速度ode45积分位置轨迹")
% figure();
% plot(t_ode,y_ode(:,4),'r--',t_ode,y_ode(:,5),'g--',t_ode,y_ode(:,6),'b--','LineWidth',1.5);hold on;
% legend('vx','vy','vz')
% title("线性插值加速度ode45积分速度轨迹")
% % 优化误差
% figure();
% plot(t_ode,y_ode(:,1)-interp1(t_opt,r_opt(1,:),t_ode),'r',t_ode,abs(y_ode(:,2)-interp1(t_opt,r_opt(2,:),t_ode)),'g',t_ode,abs(y_ode(:,3)-interp1(t_opt,r_opt(3,:),t_ode)),'b','LineWidth',1.5);hold on;
% title("线性插值加速度ode45积分位置轨迹误差")
% legend('x_{err}','y_{err}','z_{err}')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
function [dx] = dof3_ode(t,x,t_opt,a_opt)
    % 3自由度动力学方程
    dx = zeros(6,1);
    dx(1:3) = x(4:6);
    % 将优化出来的加速度按时间插值赋值
    dx(4) = interp1(t_opt,a_opt(1,:),t);
    dx(5) = interp1(t_opt,a_opt(2,:),t);
    dx(6) = interp1(t_opt,a_opt(3,:),t);
    
end


%%


%%%%%%%%%%%%%%函数部分%%%%%%%%%%%%%%%%%%%%%%%%
function [r,v,a]=calculate_trajectory(coeff,t,PDG_TFC_parmeters)
% global PDG_TFC_constant
if size(coeff,1) < 9*PDG_TFC_constant.Chebyshev_N+12
    error('coeff size error!');
end
% 共用基函数
basis_0_r = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,-1);
basis_d0_r = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,-1,1);
basis_f_r = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,1);
basis_df_r = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,1,1);
% 计算第一段最优轨迹
r1_r = zeros(3,PDG_TFC_constant.discretize_points);
v1_r = zeros(3,PDG_TFC_constant.discretize_points);
a1_r = zeros(3,PDG_TFC_constant.discretize_points);
h10x_r = basis_0_r*coeff(1:PDG_TFC_constant.Chebyshev_N);
h10x_d1_r = basis_d0_r*coeff(1:PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
h1fx_r = basis_f_r*coeff(1:PDG_TFC_constant.Chebyshev_N);
h1fx_d1_r = basis_df_r*coeff(1:PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
h10y_r = basis_0_r*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N);
h10y_d1_r = basis_d0_r*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
h1fy_r = basis_f_r*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N);
h1fy_d1_r = basis_df_r*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
h10z_r = basis_0_r*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N);
h10z_d1_r = basis_d0_r*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
h1fz_r = basis_f_r*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N);
h1fz_d1_r = basis_df_r*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
for i = 1:1:PDG_TFC_constant.discretize_points
    % 计算位置
    switch1_f_r1 = tfc_constrain_table(t(i),'.','Initial_Value',1,PDG_TFC_parmeters);
    switch1_f_r2 = tfc_constrain_table(t(i),'.','Final_Value',1,PDG_TFC_parmeters);
    switch1_f_r3 = tfc_constrain_table(t(i),'.','Initial_Derivative',1,PDG_TFC_parmeters);
    switch1_f_r4 = tfc_constrain_table(t(i),'.','Final_Derivative',1,PDG_TFC_parmeters);
    basis1_r = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t0_guess)./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess).*2.-1);
    h1x = basis1_r*coeff(1:PDG_TFC_constant.Chebyshev_N);
    h1y = basis1_r*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N);
    h1z = basis1_r*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N);
    r1_r(:,i) = [h1x - switch1_f_r1*(h10x_r-PDG_TFC_constant.r0(1)) - ...
        switch1_f_r2*(h1fx_r-coeff(9*PDG_TFC_constant.Chebyshev_N+1,1)) - ...
            switch1_f_r3*(h10x_d1_r-PDG_TFC_constant.v0(1)) - ...
                switch1_f_r4*(h1fx_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+4,1));
                h1y - switch1_f_r1*(h10y_r-PDG_TFC_constant.r0(2)) - ...
                    switch1_f_r2*(h1fy_r-coeff(9*PDG_TFC_constant.Chebyshev_N+2,1)) - ...
                        switch1_f_r3*(h10y_d1_r-PDG_TFC_constant.v0(2)) - ...
                            switch1_f_r4*(h1fy_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+5,1));
                h1z - switch1_f_r1*(h10z_r-PDG_TFC_constant.r0(3)) - ...
                    switch1_f_r2*(h1fz_r-coeff(9*PDG_TFC_constant.Chebyshev_N+3,1)) - ...
                        switch1_f_r3*(h10z_d1_r-PDG_TFC_constant.v0(3)) - ...
                            switch1_f_r4*(h1fz_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+6,1))];
        % 计算速度
        switch1_f_v_r1 = tfc_constrain_table(t(i),'d/dt','Initial_Value',1,PDG_TFC_parmeters);
        switch1_f_v_r2 = tfc_constrain_table(t(i),'d/dt','Final_Value',1,PDG_TFC_parmeters);
        switch1_f_v_r3 = tfc_constrain_table(t(i),'d/dt','Initial_Derivative',1,PDG_TFC_parmeters);
        switch1_f_v_r4 = tfc_constrain_table(t(i),'d/dt','Final_Derivative',1,PDG_TFC_parmeters);
        basis1_v_r = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t0_guess)./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess).*2.-1,1);
        h1x_d1 = basis1_v_r*coeff(1:PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
        h1y_d1 = basis1_v_r*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
        h1z_d1 = basis1_v_r*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
        v1_r(:,i) = [h1x_d1 - switch1_f_v_r1*(h10x_r-PDG_TFC_constant.r0(1)) - ...
            switch1_f_v_r2*(h1fx_r-coeff(9*PDG_TFC_constant.Chebyshev_N+1,1)) - ...
                switch1_f_v_r3*(h10x_d1_r-PDG_TFC_constant.v0(1)) - ...
                    switch1_f_v_r4*(h1fx_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+4,1));
                    h1y_d1 - switch1_f_v_r1*(h10y_r-PDG_TFC_constant.r0(2)) - ...
                        switch1_f_v_r2*(h1fy_r-coeff(9*PDG_TFC_constant.Chebyshev_N+2,1)) - ...
                            switch1_f_v_r3*(h10y_d1_r-PDG_TFC_constant.v0(2)) - ...
                                switch1_f_v_r4*(h1fy_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+5,1));
                    h1z_d1 - switch1_f_v_r1*(h10z_r-PDG_TFC_constant.r0(3)) - ...
                        switch1_f_v_r2*(h1fz_r-coeff(9*PDG_TFC_constant.Chebyshev_N+3,1)) - ...
                            switch1_f_v_r3*(h10z_d1_r-PDG_TFC_constant.v0(3)) - ...
                                switch1_f_v_r4*(h1fz_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+6,1))];
        % 计算加速度
        switch1_f_a_r1 = tfc_constrain_table(t(i),'d^2/dt^2','Initial_Value',1,PDG_TFC_parmeters);
        switch1_f_a_r2 = tfc_constrain_table(t(i),'d^2/dt^2','Final_Value',1,PDG_TFC_parmeters);
        switch1_f_a_r3 = tfc_constrain_table(t(i),'d^2/dt^2','Initial_Derivative',1,PDG_TFC_parmeters);
        switch1_f_a_r4 = tfc_constrain_table(t(i),'d^2/dt^2','Final_Derivative',1,PDG_TFC_parmeters);
        basis1_a_r = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t0_guess)./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess).*2.-1,2);
        h1x_d2 = basis1_a_r*coeff(1:PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess))^2;
        h1y_d2 = basis1_a_r*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess))^2;
        h1z_d2 = basis1_a_r*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess))^2;
        a1_r(:,i) = [h1x_d2 - switch1_f_a_r1*(h10x_r-PDG_TFC_constant.r0(1)) - ...
            switch1_f_a_r2*(h1fx_r-coeff(9*PDG_TFC_constant.Chebyshev_N+1,1)) - ...
                switch1_f_a_r3*(h10x_d1_r-PDG_TFC_constant.v0(1)) - ...
                    switch1_f_a_r4*(h1fx_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+4,1));
                    h1y_d2 - switch1_f_a_r1*(h10y_r-PDG_TFC_constant.r0(2)) - ...
                        switch1_f_a_r2*(h1fy_r-coeff(9*PDG_TFC_constant.Chebyshev_N+2,1)) - ...
                            switch1_f_a_r3*(h10y_d1_r-PDG_TFC_constant.v0(2)) - ...
                                switch1_f_a_r4*(h1fy_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+5,1));
                    h1z_d2 - switch1_f_a_r1*(h10z_r-PDG_TFC_constant.r0(3)) - ...
                        switch1_f_a_r2*(h1fz_r-coeff(9*PDG_TFC_constant.Chebyshev_N+3,1)) - ...
                            switch1_f_a_r3*(h10z_d1_r-PDG_TFC_constant.v0(3)) - ...
                                switch1_f_a_r4*(h1fz_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+6,1))];
end
% 计算第二段最优轨迹
r2_r = zeros(3,PDG_TFC_constant.discretize_points);
v2_r = zeros(3,PDG_TFC_constant.discretize_points);
a2_r = zeros(3,PDG_TFC_constant.discretize_points);
h20x_r = basis_0_r*coeff(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N);
h20x_d1_r = basis_d0_r*coeff(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
h2fx_r = basis_f_r*coeff(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N);
h2fx_d1_r = basis_df_r*coeff(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
h20y_r = basis_0_r*coeff(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N);
h20y_d1_r = basis_d0_r*coeff(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
h2fy_r = basis_f_r*coeff(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N);
h2fy_d1_r = basis_df_r*coeff(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
h20z_r = basis_0_r*coeff(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N);
h20z_d1_r = basis_d0_r*coeff(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
h2fz_r = basis_f_r*coeff(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N);
h2fz_d1_r = basis_df_r*coeff(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
for i=1:1:PDG_TFC_constant.discretize_points
    % 计算位置
    switch2_f_r1 = tfc_constrain_table(t(i+PDG_TFC_constant.discretize_points),'.','Initial_Value',2,PDG_TFC_parmeters);
    switch2_f_r2 = tfc_constrain_table(t(i+PDG_TFC_constant.discretize_points),'.','Final_Value',2,PDG_TFC_parmeters);
    switch2_f_r3 = tfc_constrain_table(t(i+PDG_TFC_constant.discretize_points),'.','Initial_Derivative',2,PDG_TFC_parmeters);
    switch2_f_r4 = tfc_constrain_table(t(i+PDG_TFC_constant.discretize_points),'.','Final_Derivative',2,PDG_TFC_parmeters);
    basis2_r = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,(t(i+PDG_TFC_constant.discretize_points)-PDG_TFC_parmeters.t1)./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1).*2.-1);
    h2x = basis2_r*coeff(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N);
    h2y = basis2_r*coeff(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N);
    h2z = basis2_r*coeff(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N);
    r2_r(:,i) = [h2x - switch2_f_r1*(h20x_r-coeff(9*PDG_TFC_constant.Chebyshev_N+1,1)) - ...
        switch2_f_r2*(h2fx_r-coeff(9*PDG_TFC_constant.Chebyshev_N+7,1)) - ...
            switch2_f_r3*(h20x_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+4,1)) - ...
                switch2_f_r4*(h2fx_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+10,1));
                h2y - switch2_f_r1*(h20y_r-coeff(9*PDG_TFC_constant.Chebyshev_N+2,1)) - ...
                    switch2_f_r2*(h2fy_r-coeff(9*PDG_TFC_constant.Chebyshev_N+8,1)) - ...
                        switch2_f_r3*(h20y_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+5,1)) - ...
                            switch2_f_r4*(h2fy_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+11,1));
                h2z - switch2_f_r1*(h20z_r-coeff(9*PDG_TFC_constant.Chebyshev_N+3,1)) - ...
                    switch2_f_r2*(h2fz_r-coeff(9*PDG_TFC_constant.Chebyshev_N+9,1)) - ...
                        switch2_f_r3*(h20z_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+6,1)) - ...
                            switch2_f_r4*(h2fz_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+12,1))];
    % 计算速度
    switch2_f_v_r1 = tfc_constrain_table(t(i+PDG_TFC_constant.discretize_points),'d/dt','Initial_Value',2,PDG_TFC_parmeters);
    switch2_f_v_r2 = tfc_constrain_table(t(i+PDG_TFC_constant.discretize_points),'d/dt','Final_Value',2,PDG_TFC_parmeters);
    switch2_f_v_r3 = tfc_constrain_table(t(i+PDG_TFC_constant.discretize_points),'d/dt','Initial_Derivative',2,PDG_TFC_parmeters);
    switch2_f_v_r4 = tfc_constrain_table(t(i+PDG_TFC_constant.discretize_points),'d/dt','Final_Derivative',2,PDG_TFC_parmeters);
    basis2_v_r = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,(t(i+PDG_TFC_constant.discretize_points)-PDG_TFC_parmeters.t1)./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1).*2.-1,1);
    h2x_d1 = basis2_v_r*coeff(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
    h2y_d1 = basis2_v_r*coeff(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
    h2z_d1 = basis2_v_r*coeff(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
    v2_r(:,i) = [h2x_d1 - switch2_f_v_r1*(h20x_r-coeff(9*PDG_TFC_constant.Chebyshev_N+1,1)) - ...
        switch2_f_v_r2*(h2fx_r-coeff(9*PDG_TFC_constant.Chebyshev_N+7,1)) - ...
            switch2_f_v_r3*(h20x_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+4,1)) - ...
                switch2_f_v_r4*(h2fx_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+10,1));
                h2y_d1 - switch2_f_v_r1*(h20y_r-coeff(9*PDG_TFC_constant.Chebyshev_N+2,1)) - ...
                    switch2_f_v_r2*(h2fy_r-coeff(9*PDG_TFC_constant.Chebyshev_N+8,1)) - ...
                        switch2_f_v_r3*(h20y_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+5,1)) - ...
                            switch2_f_v_r4*(h2fy_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+11,1));
                h2z_d1 - switch2_f_v_r1*(h20z_r-coeff(9*PDG_TFC_constant.Chebyshev_N+3,1)) - ...
                    switch2_f_v_r2*(h2fz_r-coeff(9*PDG_TFC_constant.Chebyshev_N+9,1)) - ...
                        switch2_f_v_r3*(h20z_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+6,1)) - ...
                            switch2_f_v_r4*(h2fz_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+12,1))];
    % 加速度计算
    switch2_f_a_r1 = tfc_constrain_table(t(i+PDG_TFC_constant.discretize_points),'d^2/dt^2','Initial_Value',2,PDG_TFC_parmeters);
    switch2_f_a_r2 = tfc_constrain_table(t(i+PDG_TFC_constant.discretize_points),'d^2/dt^2','Final_Value',2,PDG_TFC_parmeters);
    switch2_f_a_r3 = tfc_constrain_table(t(i+PDG_TFC_constant.discretize_points),'d^2/dt^2','Initial_Derivative',2,PDG_TFC_parmeters);
    switch2_f_a_r4 = tfc_constrain_table(t(i+PDG_TFC_constant.discretize_points),'d^2/dt^2','Final_Derivative',2,PDG_TFC_parmeters);
    basis2_a_r = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,(t(i+PDG_TFC_constant.discretize_points)-PDG_TFC_parmeters.t1)./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1).*2.-1,2);
    h2x_d2 = basis2_a_r*coeff(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1))^2;
    h2y_d2 = basis2_a_r*coeff(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1))^2;
    h2z_d2 = basis2_a_r*coeff(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1))^2;
    a2_r(:,i) = [h2x_d2 - switch2_f_a_r1*(h20x_r-coeff(9*PDG_TFC_constant.Chebyshev_N+1,1)) - ...
        switch2_f_a_r2*(h2fx_r-coeff(9*PDG_TFC_constant.Chebyshev_N+7,1)) - ...
            switch2_f_a_r3*(h20x_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+4,1)) - ...
                switch2_f_a_r4*(h2fx_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+10,1));
                h2y_d2 - switch2_f_a_r1*(h20y_r-coeff(9*PDG_TFC_constant.Chebyshev_N+2,1)) - ...
                    switch2_f_a_r2*(h2fy_r-coeff(9*PDG_TFC_constant.Chebyshev_N+8,1)) - ...
                        switch2_f_a_r3*(h20y_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+5,1)) - ...
                            switch2_f_a_r4*(h2fy_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+11,1));
                h2z_d2 - switch2_f_a_r1*(h20z_r-coeff(9*PDG_TFC_constant.Chebyshev_N+3,1)) - ...
                    switch2_f_a_r2*(h2fz_r-coeff(9*PDG_TFC_constant.Chebyshev_N+9,1)) - ...
                        switch2_f_a_r3*(h20z_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+6,1)) - ...
                            switch2_f_a_r4*(h2fz_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+12,1))];
end
% 计算第三段最优轨迹
r3_r = zeros(3,PDG_TFC_constant.discretize_points);
v3_r = zeros(3,PDG_TFC_constant.discretize_points);
a3_r = zeros(3,PDG_TFC_constant.discretize_points);
h30x_r = basis_0_r*coeff(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N);
h30x_d1_r = basis_d0_r*coeff(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
h3fx_r = basis_f_r*coeff(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N);
h3fx_d1_r = basis_df_r*coeff(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
h30y_r = basis_0_r*coeff(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N);
h30y_d1_r = basis_d0_r*coeff(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
h3fy_r = basis_f_r*coeff(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N);
h3fy_d1_r = basis_df_r*coeff(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
h30z_r = basis_0_r*coeff(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N);
h30z_d1_r = basis_d0_r*coeff(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
h3fz_r = basis_f_r*coeff(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N);
h3fz_d1_r = basis_df_r*coeff(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
for i=1:1:PDG_TFC_constant.discretize_points
    % 计算位置
    switch3_f_r1 = tfc_constrain_table(t(i+2*PDG_TFC_constant.discretize_points),'.','Initial_Value',3,PDG_TFC_parmeters);
    switch3_f_r2 = tfc_constrain_table(t(i+2*PDG_TFC_constant.discretize_points),'.','Final_Value',3,PDG_TFC_parmeters);
    switch3_f_r3 = tfc_constrain_table(t(i+2*PDG_TFC_constant.discretize_points),'.','Initial_Derivative',3,PDG_TFC_parmeters);
    switch3_f_r4 = tfc_constrain_table(t(i+2*PDG_TFC_constant.discretize_points),'.','Final_Derivative',3,PDG_TFC_parmeters);
    basis3_r = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,(t(i+2*PDG_TFC_constant.discretize_points)-PDG_TFC_parmeters.t2)./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2).*2.-1);
    h3x = basis3_r*coeff(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N);
    h3y = basis3_r*coeff(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N);
    h3z = basis3_r*coeff(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N);
    r3_r(:,i) = [h3x - switch3_f_r1*(h30x_r-coeff(9*PDG_TFC_constant.Chebyshev_N+7,1)) - ...
        switch3_f_r2*(h3fx_r-PDG_TFC_constant.rf(1)) - ...
            switch3_f_r3*(h30x_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+10,1)) - ...
                switch3_f_r4*(h3fx_d1_r-PDG_TFC_constant.vf(1));
                h3y - switch3_f_r1*(h30y_r-coeff(9*PDG_TFC_constant.Chebyshev_N+8,1)) - ...
                    switch3_f_r2*(h3fy_r-PDG_TFC_constant.rf(2)) - ...
                        switch3_f_r3*(h30y_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+11,1)) - ...
                            switch3_f_r4*(h3fy_d1_r-PDG_TFC_constant.vf(2));
                h3z - switch3_f_r1*(h30z_r-coeff(9*PDG_TFC_constant.Chebyshev_N+9,1)) - ...
                    switch3_f_r2*(h3fz_r-PDG_TFC_constant.rf(3)) - ...
                        switch3_f_r3*(h30z_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+12,1)) - ...
                            switch3_f_r4*(h3fz_d1_r-PDG_TFC_constant.vf(3))];
    % 计算速度
    switch3_f_v_r1 = tfc_constrain_table(t(i+2*PDG_TFC_constant.discretize_points),'d/dt','Initial_Value',3,PDG_TFC_parmeters);
    switch3_f_v_r2 = tfc_constrain_table(t(i+2*PDG_TFC_constant.discretize_points),'d/dt','Final_Value',3,PDG_TFC_parmeters);
    switch3_f_v_r3 = tfc_constrain_table(t(i+2*PDG_TFC_constant.discretize_points),'d/dt','Initial_Derivative',3,PDG_TFC_parmeters);
    switch3_f_v_r4 = tfc_constrain_table(t(i+2*PDG_TFC_constant.discretize_points),'d/dt','Final_Derivative',3,PDG_TFC_parmeters);
    basis3_v_r = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,(t(i+2*PDG_TFC_constant.discretize_points)-PDG_TFC_parmeters.t2)./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2).*2.-1,1);
    h3x_d1 = basis3_v_r*coeff(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
    h3y_d1 = basis3_v_r*coeff(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
    h3z_d1 = basis3_v_r*coeff(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
    v3_r(:,i) = [h3x_d1 - switch3_f_v_r1*(h30x_r-coeff(9*PDG_TFC_constant.Chebyshev_N+7,1)) - ...
        switch3_f_v_r2*(h3fx_r-PDG_TFC_constant.rf(1)) - ...
            switch3_f_v_r3*(h30x_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+10,1)) - ...
                switch3_f_v_r4*(h3fx_d1_r-PDG_TFC_constant.vf(1));
                h3y_d1 - switch3_f_v_r1*(h30y_r-coeff(9*PDG_TFC_constant.Chebyshev_N+8,1)) - ...
                    switch3_f_v_r2*(h3fy_r-PDG_TFC_constant.rf(2)) - ...
                        switch3_f_v_r3*(h30y_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+11,1)) - ...
                            switch3_f_v_r4*(h3fy_d1_r-PDG_TFC_constant.vf(2));
                h3z_d1 - switch3_f_v_r1*(h30z_r-coeff(9*PDG_TFC_constant.Chebyshev_N+9,1)) - ...
                    switch3_f_v_r2*(h3fz_r-PDG_TFC_constant.rf(3)) - ...
                        switch3_f_v_r3*(h30z_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+12,1)) - ...
                            switch3_f_v_r4*(h3fz_d1_r-PDG_TFC_constant.vf(3))];
    % 加速度计算
    switch3_f_a_r1 = tfc_constrain_table(t(i+2*PDG_TFC_constant.discretize_points),'d^2/dt^2','Initial_Value',3,PDG_TFC_parmeters);
    switch3_f_a_r2 = tfc_constrain_table(t(i+2*PDG_TFC_constant.discretize_points),'d^2/dt^2','Final_Value',3,PDG_TFC_parmeters);
    switch3_f_a_r3 = tfc_constrain_table(t(i+2*PDG_TFC_constant.discretize_points),'d^2/dt^2','Initial_Derivative',3,PDG_TFC_parmeters);
    switch3_f_a_r4 = tfc_constrain_table(t(i+2*PDG_TFC_constant.discretize_points),'d^2/dt^2','Final_Derivative',3,PDG_TFC_parmeters);
    basis3_a_r = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,(t(i+2*PDG_TFC_constant.discretize_points)-PDG_TFC_parmeters.t2)./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2).*2.-1,2);
    h3x_d2 = basis3_a_r*coeff(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2))^2;
    h3y_d2 = basis3_a_r*coeff(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2))^2;
    h3z_d2 = basis3_a_r*coeff(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2))^2;
    a3_r(:,i) = [h3x_d2 - switch3_f_a_r1*(h30x_r-coeff(9*PDG_TFC_constant.Chebyshev_N+7,1)) - ...
        switch3_f_a_r2*(h3fx_r-PDG_TFC_constant.rf(1)) - ...
            switch3_f_a_r3*(h30x_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+10,1)) - ...
                switch3_f_a_r4*(h3fx_d1_r-PDG_TFC_constant.vf(1));
                h3y_d2 - switch3_f_a_r1*(h30y_r-coeff(9*PDG_TFC_constant.Chebyshev_N+8,1)) - ...
                    switch3_f_a_r2*(h3fy_r-PDG_TFC_constant.rf(2)) - ...
                        switch3_f_a_r3*(h30y_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+11,1)) - ...
                            switch3_f_a_r4*(h3fy_d1_r-PDG_TFC_constant.vf(2));
                h3z_d2 - switch3_f_a_r1*(h30z_r-coeff(9*PDG_TFC_constant.Chebyshev_N+9,1)) - ...
                    switch3_f_a_r2*(h3fz_r-PDG_TFC_constant.rf(3)) - ...
                        switch3_f_a_r3*(h30z_d1_r-coeff(9*PDG_TFC_constant.Chebyshev_N+12,1)) - ...
                            switch3_f_a_r4*(h3fz_d1_r-PDG_TFC_constant.vf(3))];
end
r = [r1_r,r2_r,r3_r];
v = [v1_r,v2_r,v3_r];
a = [a1_r,a2_r,a3_r];
fprintf("x1x:%.4f,x1y:%.4f,x1z:%.4f,v1x:%.4f,v1y:%.4f,v1z:%.4f\n",coeff(9*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N+6,1));
fprintf("x2x:%.4f,x2y:%.4f,x2z:%.4f,v2x:%.4f,v2y:%.4f,v2z:%.4f\n",coeff(9*PDG_TFC_constant.Chebyshev_N+7:9*PDG_TFC_constant.Chebyshev_N+12,1));
end



% 计算lamda_m 协态,在求解完轨迹之后计算质量协态 目前协态的切比雪夫多项式与轨迹采用相同的阶数
% coeff 为协态的系数
function [residual] = PDG_L_lamda_m(coeff,t,lamda_v,PDG_TFC_parmeters)
    % global PDG_TFC_constant
    residual = zeros(3*PDG_TFC_constant.discretize_points,1);
    % 第一段质量协态
    for i = 1:1:PDG_TFC_constant.discretize_points
        %basis1_f_lamda_m = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,1);
        %basis1_lamda_m = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t0_guess)./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess).*2.-1);
        % h1_lamda_m = basis1_lamda_m*coeff(1:PDG_TFC_constant.Chebyshev_N);
        % hf_lamda_m = basis1_f_lamda_m*coeff(1:PDG_TFC_constant.Chebyshev_N);
        basis1_lamda_d1_m = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t0_guess)./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess).*2.-1,1);
        h1_d1_lamda_m = basis1_lamda_d1_m*coeff(1:PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
        residual(i,1) = h1_d1_lamda_m + PDG_TFC_constant.Tmax/(PDG_TFC_constant.m0-PDG_TFC_constant.alpha *PDG_TFC_constant.Tmax*t(i))*norm(lamda_v(1:3,i),2);
    end
    for i = PDG_TFC_constant.discretize_points+1:1:2*PDG_TFC_constant.discretize_points
        basis2_lamda_d1_m = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t1)./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1).*2.-1,1);
        h2_d1_lamda_m = basis2_lamda_d1_m*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
        residual(i,1) = h2_d1_lamda_m + PDG_TFC_constant.Tmin/(PDG_TFC_constant.m0 ...
            -PDG_TFC_constant.alpha * ...
            (PDG_TFC_constant.Tmin*(t(i)-PDG_TFC_parmeters.t1) ...
            +PDG_TFC_constant.Tmax*(PDG_TFC_parmeters.t1))...
        )*norm(lamda_v(1:3,i),2);
    end
    for i = 2*PDG_TFC_constant.discretize_points+1:1:3*PDG_TFC_constant.discretize_points
        basis3_lamda_d1_m = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t2)./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2).*2.-1,1);
        h3_d1_lamda_m = basis3_lamda_d1_m*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
        residual(i,1) = h3_d1_lamda_m + PDG_TFC_constant.Tmax/(PDG_TFC_constant.m0 ...
            -PDG_TFC_constant.alpha ...
            *(PDG_TFC_constant.Tmax*(t(i)-PDG_TFC_parmeters.t2)...
            +PDG_TFC_constant.Tmin*(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1)...
            +PDG_TFC_constant.Tmax*(PDG_TFC_parmeters.t1))...
            )*norm(lamda_v(1:3,i),2);
    end

end
function [lamda_m_r] = calculate_lamda_m(coeff,t,PDG_TFC_parmeters)
    % global PDG_TFC_constant
    lamda_m_r = zeros(1,3*PDG_TFC_constant.discretize_points);
    % 由于协态方程是一个反向传播问题，因此从第三段开始计算
    basis_f_lamda_m = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,1);
    % 第三段
    for i=2*PDG_TFC_constant.discretize_points+1:1:3*PDG_TFC_constant.discretize_points
        basis3_lamda_m = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t2)./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2).*2.-1);
        %basis3_f_lamda_m = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,1);
        h3_lamda_m = basis3_lamda_m*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N);
        h3f_lamda_m = basis_f_lamda_m*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N);
        lamda_m_r(1,i) = h3_lamda_m-h3f_lamda_m + 0;
    end
    for i=PDG_TFC_constant.discretize_points+1:1:2*PDG_TFC_constant.discretize_points
        basis2_lamda_m = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t1)./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1).*2.-1);
        %basis2_f_lamda_m = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,1);
        h2_lamda_m = basis2_lamda_m*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N);
        h2f_lamda_m = basis_f_lamda_m*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N);
        lamda_m_r(1,i) = h2_lamda_m-h2f_lamda_m + lamda_m_r(1,2*PDG_TFC_constant.discretize_points+1);
    end
    for i=1:1:PDG_TFC_constant.discretize_points
        basis1_lamda_m = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t0_guess)./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess).*2.-1);
        %basis1_f_lamda_m = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,1);
        h1_lamda_m = basis1_lamda_m*coeff(1:PDG_TFC_constant.Chebyshev_N);
        h1f_lamda_m = basis_f_lamda_m*coeff(1:PDG_TFC_constant.Chebyshev_N);
        lamda_m_r(1,i) = h1_lamda_m-h1f_lamda_m + lamda_m_r(1,PDG_TFC_constant.discretize_points+1);
    end
end



% 计算残差的函数
function [residuals] = PDG_3stage(coeff,t,PDG_TFC_parmeters)%,r,v,a)
    % global PDG_TFC_constant
    residuals = zeros(9*PDG_TFC_constant.discretize_points+1,1);
    % 获取切换点时间index 升序t的index
%     index_t1 = find(t>PDG_TFC_parmeters.t1,1,'first');
%     index_t2 = find(t>PDG_TFC_parmeters.t2,1,'first');
    % 切比雪夫插值基函数(三阶段共用)
    basis_0 = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,-1);
    basis_d0 = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,-1,1);
    basis_f = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,1);
    basis_d_f = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,1,1);
    % 第一段
    % 生成h0和hf
    % x轴
    h10x = basis_0*coeff(1:PDG_TFC_constant.Chebyshev_N);
    h10x_d1 = basis_d0*coeff(1:PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess); 
    h1fx = basis_f*coeff(1:PDG_TFC_constant.Chebyshev_N);
    h1fx_d1 = basis_d_f*coeff(1:PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);% ? 存疑 是否乘 tf-t0用于变换作用域
    % y轴
    h10y = basis_0*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N);
    h10y_d1 = basis_d0*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
    h1fy = basis_f*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N);
    h1fy_d1 = basis_d_f*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);% ? 存疑 是否乘 tf-t0用于变换作用域
    % z轴
    h10z = basis_0*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N);
    h10z_d1 = basis_d0*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
    h1fz = basis_f*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N);
    h1fz_d1 = basis_d_f*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);% ? 存疑 是否乘 tf-t0用于变换作用域
    % 计算第一段 Tmax 1L
    for i=1:1:PDG_TFC_constant.discretize_points
        % 第一段 Tmax
        % 计算切比雪夫多项式基函数
        % basis = chebyshev_basis(PDG_TFC_constant.Chebyshev_N, ...
        % (t(i)-PDG_TFC_parmeters.t0_guess)./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t0_guess).*2.-1);
        % % 自由函数gx1
        % h1 = basis*coeff(1:PDG_TFC_constant.Chebyshev_N);
        % % 计算切比雪夫多项式一阶导数基函数
        % basis_d1 = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N, ...
        % (t(i)-PDG_TFC_parmeters.t0_guess)./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t0_guess).*2.-1,1);
        % h1_d1 = basis_d1*coeff(1:PDG_TFC_constant.Chebyshev_N);
        % 计算第一段切比雪夫多项式二阶导数基函数 (x,y,z)三轴
        basis1_d2 = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N, ...
        (t(i)-PDG_TFC_parmeters.t0_guess)./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess).*2.-1,2);
        % x轴
        h1x_d2 = basis1_d2*coeff(1:PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess))^2;
        % y轴
        h1y_d2 = basis1_d2*coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess))^2;
        % z轴 
        h1z_d2 = basis1_d2*coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess))^2;
        % 计算TFC约束表达式
        % 切换函数临时变量
        switch1_f_d2_t1 = tfc_constrain_table(t(i),'d^2/dt^2','Initial_Value',1,PDG_TFC_parmeters);
        switch1_f_d2_t2 = tfc_constrain_table(t(i),'d^2/dt^2','Final_Value',1,PDG_TFC_parmeters);
        switch1_f_d2_t3 = tfc_constrain_table(t(i),'d^2/dt^2','Initial_Derivative',1,PDG_TFC_parmeters);
        switch1_f_d2_t4 = tfc_constrain_table(t(i),'d^2/dt^2','Final_Derivative',1,PDG_TFC_parmeters);
        % 计算约束表达式
        a_11 = [h1x_d2 - switch1_f_d2_t1*(h10x-PDG_TFC_constant.r0(1)) - ...
            switch1_f_d2_t2*(h1fx-coeff(9*PDG_TFC_constant.Chebyshev_N+1,1)) - ...
                switch1_f_d2_t3*(h10x_d1-PDG_TFC_constant.v0(1)) - ...
                    switch1_f_d2_t4*(h1fx_d1-coeff(9*PDG_TFC_constant.Chebyshev_N+4,1));

        h1y_d2 - switch1_f_d2_t1*(h10y-PDG_TFC_constant.r0(2)) - ...
            switch1_f_d2_t2*(h1fy-coeff(9*PDG_TFC_constant.Chebyshev_N+2,1)) - ...
                switch1_f_d2_t3*(h10y_d1-PDG_TFC_constant.v0(2)) - ...
                    switch1_f_d2_t4*(h1fy_d1-coeff(9*PDG_TFC_constant.Chebyshev_N+5,1));

        h1z_d2 - switch1_f_d2_t1*(h10z-PDG_TFC_constant.r0(3)) - ...
            switch1_f_d2_t2*(h1fz-coeff(9*PDG_TFC_constant.Chebyshev_N+3,1)) - ...
                switch1_f_d2_t3*(h10z_d1-PDG_TFC_constant.v0(3)) - ...
                    switch1_f_d2_t4*(h1fz_d1-coeff(9*PDG_TFC_constant.Chebyshev_N+6,1))];

        % 计算协态变量 a0 在前 a1 在后
        
        lambda_v1 = [coeff(9*PDG_TFC_constant.Chebyshev_N+13:9*PDG_TFC_constant.Chebyshev_N+14).'*[1;t(i)];
                    coeff(9*PDG_TFC_constant.Chebyshev_N+15:9*PDG_TFC_constant.Chebyshev_N+16).'*[1;t(i)];
                    coeff(9*PDG_TFC_constant.Chebyshev_N+17:9*PDG_TFC_constant.Chebyshev_N+18).'*[1;t(i)];];
        % 计算最优轨迹
        %r_opt = TFC_mat * coeff(1:PDG_TFC_constant.Chebyshev_N);
        %v_opt = TFC_mat * coeff(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N);
        % 计算加速度
        %a_opt = TFC_mat * coeff(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N);
        % 计算残差
        residuals(3*(i-1)+1:3*i,1) = a_11 - PDG_TFC_constant.mar_g - ...
        PDG_TFC_constant.Tmax / (PDG_TFC_constant.m0-PDG_TFC_constant.alpha *PDG_TFC_constant.Tmax*t(i)) * lambda_v1/norm(lambda_v1,2);
    end
    % 第二段 Tmin 2L
        % x轴
        h20x = basis_0*coeff(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N);
        h20x_d1 = basis_d0*coeff(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
        h2fx = basis_f*coeff(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N);
        h2fx_d1 = basis_d_f*coeff(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
        % y轴
        h20y = basis_0*coeff(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N);
        h20y_d1 = basis_d0*coeff(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
        h2fy = basis_f*coeff(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N);
        h2fy_d1 = basis_d_f*coeff(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
        % z轴
        h20z = basis_0*coeff(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N);
        h20z_d1 = basis_d0*coeff(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
        h2fz = basis_f*coeff(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N);
        h2fz_d1 = basis_d_f*coeff(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
        % 计算第二段 Tmin
        for i=PDG_TFC_constant.discretize_points+1:1:2*PDG_TFC_constant.discretize_points
            basis2_d2 = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N, ...
                (t(i)-PDG_TFC_parmeters.t1)./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1).*2.-1,2);
            h2x_d2 = basis2_d2*coeff(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1))^2;
            h2y_d2 = basis2_d2*coeff(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1))^2;
            h2z_d2 = basis2_d2*coeff(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1))^2;
            switch2_f_d2_t1 = tfc_constrain_table(t(i),'d^2/dt^2','Initial_Value',2,PDG_TFC_parmeters);
            switch2_f_d2_t2 = tfc_constrain_table(t(i),'d^2/dt^2','Final_Value',2,PDG_TFC_parmeters);
            switch2_f_d2_t3 = tfc_constrain_table(t(i),'d^2/dt^2','Initial_Derivative',2,PDG_TFC_parmeters);
            switch2_f_d2_t4 = tfc_constrain_table(t(i),'d^2/dt^2','Final_Derivative',2,PDG_TFC_parmeters);
            a_22 = [h2x_d2 - switch2_f_d2_t1*(h20x-coeff(9*PDG_TFC_constant.Chebyshev_N+1,1)) - ...
                switch2_f_d2_t2*(h2fx-coeff(9*PDG_TFC_constant.Chebyshev_N+7,1)) - ...
                    switch2_f_d2_t3*(h20x_d1-coeff(9*PDG_TFC_constant.Chebyshev_N+4,1)) - ...
                        switch2_f_d2_t4*(h2fx_d1-coeff(9*PDG_TFC_constant.Chebyshev_N+10,1));

            h2y_d2 - switch2_f_d2_t1*(h20y-coeff(9*PDG_TFC_constant.Chebyshev_N+2,1)) - ...
                switch2_f_d2_t2*(h2fy-coeff(9*PDG_TFC_constant.Chebyshev_N+8,1)) - ...
                    switch2_f_d2_t3*(h20y_d1-coeff(9*PDG_TFC_constant.Chebyshev_N+5,1)) - ...
                        switch2_f_d2_t4*(h2fy_d1-coeff(9*PDG_TFC_constant.Chebyshev_N+11,1));

            h2z_d2 - switch2_f_d2_t1*(h20z-coeff(9*PDG_TFC_constant.Chebyshev_N+3,1)) - ...
                switch2_f_d2_t2*(h2fz-coeff(9*PDG_TFC_constant.Chebyshev_N+9,1)) - ...
                    switch2_f_d2_t3*(h20z_d1-coeff(9*PDG_TFC_constant.Chebyshev_N+6,1)) - ...
                        switch2_f_d2_t4*(h2fz_d1-coeff(9*PDG_TFC_constant.Chebyshev_N+12,1))];

            lambda_v2 = [coeff(9*PDG_TFC_constant.Chebyshev_N+13:9*PDG_TFC_constant.Chebyshev_N+14).'*[1;t(i)];
                        coeff(9*PDG_TFC_constant.Chebyshev_N+15:9*PDG_TFC_constant.Chebyshev_N+16).'*[1;t(i)];
                        coeff(9*PDG_TFC_constant.Chebyshev_N+17:9*PDG_TFC_constant.Chebyshev_N+18).'*[1;t(i)];];
            residuals(3*(i-1)+1:3*i,1) = a_22 - PDG_TFC_constant.mar_g - ...
            PDG_TFC_constant.Tmin / (PDG_TFC_constant.m0 ...
                                        -PDG_TFC_constant.alpha * ...
                                        (PDG_TFC_constant.Tmin*(t(i)-PDG_TFC_parmeters.t1) ...
                                        +PDG_TFC_constant.Tmax*(PDG_TFC_parmeters.t1))...
                                    ) * lambda_v2/norm(lambda_v2,2);
        end
    % 第三段 3L
        % x轴
        h30x = basis_0*coeff(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N);
        h30x_d1 = basis_d0*coeff(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
        h3fx = basis_f*coeff(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N);
        h3fx_d1 = basis_d_f*coeff(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
        % y轴
        h30y = basis_0*coeff(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N);
        h30y_d1 = basis_d0*coeff(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
        h3fy = basis_f*coeff(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N);
        h3fy_d1 = basis_d_f*coeff(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
        % z轴
        h30z = basis_0*coeff(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N);
        h30z_d1 = basis_d0*coeff(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
        h3fz = basis_f*coeff(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N);
        h3fz_d1 = basis_d_f*coeff(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
        % 计算第三段 Tmax
        for i=2*PDG_TFC_constant.discretize_points+1:1:3*PDG_TFC_constant.discretize_points
            basis3_d2 = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N, ...
                (t(i)-PDG_TFC_parmeters.t2)./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2).*2.-1,2);
            h3x_d2 = basis3_d2*coeff(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2))^2;
            h3y_d2 = basis3_d2*coeff(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2))^2;
            h3z_d2 = basis3_d2*coeff(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N)*(2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2))^2;
            switch3_f_d2_t1 = tfc_constrain_table(t(i),'d^2/dt^2','Initial_Value',3,PDG_TFC_parmeters);
            switch3_f_d2_t2 = tfc_constrain_table(t(i),'d^2/dt^2','Final_Value',3,PDG_TFC_parmeters);
            switch3_f_d2_t3 = tfc_constrain_table(t(i),'d^2/dt^2','Initial_Derivative',3,PDG_TFC_parmeters);
            switch3_f_d2_t4 = tfc_constrain_table(t(i),'d^2/dt^2','Final_Derivative',3,PDG_TFC_parmeters);
            a_33 = [h3x_d2 - switch3_f_d2_t1*(h30x-coeff(9*PDG_TFC_constant.Chebyshev_N+7,1)) - ...
                switch3_f_d2_t2*(h3fx-PDG_TFC_constant.rf(1)) - ...
                    switch3_f_d2_t3*(h30x_d1-coeff(9*PDG_TFC_constant.Chebyshev_N+10,1)) - ...
                        switch3_f_d2_t4*(h3fx_d1-PDG_TFC_constant.vf(1));

            h3y_d2 - switch3_f_d2_t1*(h30y-coeff(9*PDG_TFC_constant.Chebyshev_N+8,1)) - ...
                switch3_f_d2_t2*(h3fy-PDG_TFC_constant.rf(2)) - ...
                    switch3_f_d2_t3*(h30y_d1-coeff(9*PDG_TFC_constant.Chebyshev_N+11,1)) - ...
                        switch3_f_d2_t4*(h3fy_d1-PDG_TFC_constant.vf(2));

            h3z_d2 - switch3_f_d2_t1*(h30z-coeff(9*PDG_TFC_constant.Chebyshev_N+9,1)) - ...
                switch3_f_d2_t2*(h3fz-PDG_TFC_constant.rf(3)) - ...
                    switch3_f_d2_t3*(h30z_d1-coeff(9*PDG_TFC_constant.Chebyshev_N+12,1)) - ...
                        switch3_f_d2_t4*(h3fz_d1-PDG_TFC_constant.vf(3))];

            lambda_v3 = [coeff(9*PDG_TFC_constant.Chebyshev_N+13:9*PDG_TFC_constant.Chebyshev_N+14).'*[1;t(i)];
            coeff(9*PDG_TFC_constant.Chebyshev_N+15:9*PDG_TFC_constant.Chebyshev_N+16).'*[1;t(i)];
            coeff(9*PDG_TFC_constant.Chebyshev_N+17:9*PDG_TFC_constant.Chebyshev_N+18).'*[1;t(i)];];

            residuals(3*(i-1)+1:3*i,1) = a_33 - PDG_TFC_constant.mar_g - ...
            PDG_TFC_constant.Tmax / (PDG_TFC_constant.m0 ...
                                    -PDG_TFC_constant.alpha ...
                                    *(PDG_TFC_constant.Tmax*(t(i)-PDG_TFC_parmeters.t2)...
                                    +PDG_TFC_constant.Tmin*(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1)...
                                    +PDG_TFC_constant.Tmax*(PDG_TFC_parmeters.t1))...
                                    ) * lambda_v3/norm(lambda_v3,2);
        end
        % 计算哈密顿函数(可能存在问题)
        lambda_vf = [coeff(9*PDG_TFC_constant.Chebyshev_N+13:9*PDG_TFC_constant.Chebyshev_N+14).'*[1;PDG_TFC_parmeters.tf_guess];
        coeff(9*PDG_TFC_constant.Chebyshev_N+15:9*PDG_TFC_constant.Chebyshev_N+16).'*[1;PDG_TFC_parmeters.tf_guess];
        coeff(9*PDG_TFC_constant.Chebyshev_N+17:9*PDG_TFC_constant.Chebyshev_N+18).'*[1;PDG_TFC_parmeters.tf_guess];];
        residuals(9*PDG_TFC_constant.discretize_points+1,1) = PDG_TFC_constant.alpha * PDG_TFC_constant.Tmax...
            + PDG_TFC_constant.mar_g.'* lambda_vf - PDG_TFC_constant.Tmax / (PDG_TFC_constant.m0 ...
                                    -PDG_TFC_constant.alpha ...
                                    *(PDG_TFC_constant.Tmax*(PDG_TFC_parmeters.tf_guess - PDG_TFC_parmeters.t2)...
                                    +PDG_TFC_constant.Tmin*(PDG_TFC_parmeters.t2 - PDG_TFC_parmeters.t1)...
                                    +PDG_TFC_constant.Tmax*(PDG_TFC_parmeters.t1 - PDG_TFC_parmeters.t0_guess))...
                                    ) * norm(lambda_vf,2);
end


% 初始化切比雪夫多项式参数
% 利用非线性最小二乘法获取初始猜测
function [residual] = coeff_init_cal(coeff_init,t,r_i,v_i,PDG_TFC_parmeters)
    % global PDG_TFC_constant
    lamda_v0 = v_i(:,1)/norm(v_i(1:3,1),2);
    lamda_vf = -r_i(:,1)/norm(r_i(1:3,1),2);
    % 3段离散点速度和位置残差+6个协态变量残差(初值三个,终值三个) = 6*(3*PDG_TFC_constant.discretize_points+1)
    residual = zeros(6,3*PDG_TFC_constant.discretize_points+1); 
    %r_i(:,PDG_TFC_constant.discretize_points)=r_i(:,PDG_TFC_constant.discretize_points+1);
    %v_i(:,PDG_TFC_constant.discretize_points)=v_i(:,PDG_TFC_constant.discretize_points+1);
    %r_i(:,2*PDG_TFC_constant.discretize_points)=r_i(:,2*PDG_TFC_constant.discretize_points+1);
    %v_i(:,2*PDG_TFC_constant.discretize_points)=v_i(:,2*PDG_TFC_constant.discretize_points+1);
    % 共用基函数
    basis_0_i = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,-1);
    basis_d0_i = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,-1,1);
    basis_f_i = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,1);
    basis_df_i = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,1,1);
    % 计算第一段最优轨迹
    %r1_i = zeros(3,PDG_TFC_constant.discretize_points);
    h10x_i = basis_0_i*coeff_init(1:PDG_TFC_constant.Chebyshev_N);
    h10x_d1_i = basis_d0_i*coeff_init(1:PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
    h1fx_i = basis_f_i*coeff_init(1:PDG_TFC_constant.Chebyshev_N);
    h1fx_d1_i = basis_df_i*coeff_init(1:PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
    h10y_i = basis_0_i*coeff_init(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N);
    h10y_d1_i = basis_d0_i*coeff_init(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
    h1fy_i = basis_f_i*coeff_init(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N);
    h1fy_d1_i = basis_df_i*coeff_init(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
    h10z_i = basis_0_i*coeff_init(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N);
    h10z_d1_i = basis_d0_i*coeff_init(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
    h1fz_i = basis_f_i*coeff_init(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N);
    h1fz_d1_i = basis_df_i*coeff_init(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
    r1_i = r_i(1:3,1:PDG_TFC_constant.discretize_points);
    v1_i = v_i(1:3,1:PDG_TFC_constant.discretize_points);
    % 计算第一段最优轨迹
    for i = 1:1:PDG_TFC_constant.discretize_points
        switch1_f_i1 = tfc_constrain_table(t(i),'.','Initial_Value',1,PDG_TFC_parmeters);
        switch1_f_i2 = tfc_constrain_table(t(i),'.','Final_Value',1,PDG_TFC_parmeters);
        switch1_f_i3 = tfc_constrain_table(t(i),'.','Initial_Derivative',1,PDG_TFC_parmeters);
        switch1_f_i4 = tfc_constrain_table(t(i),'.','Final_Derivative',1,PDG_TFC_parmeters);
        switch1_f_d1_i1 = tfc_constrain_table(t(i),'d/dt','Initial_Value',1,PDG_TFC_parmeters);
        switch1_f_d1_i2 = tfc_constrain_table(t(i),'d/dt','Final_Value',1,PDG_TFC_parmeters);
        switch1_f_d1_i3 = tfc_constrain_table(t(i),'d/dt','Initial_Derivative',1,PDG_TFC_parmeters);
        switch1_f_d1_i4 = tfc_constrain_table(t(i),'d/dt','Final_Derivative',1,PDG_TFC_parmeters);
        basis1_i = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t0_guess)./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess).*2.-1);
        h1x_i = basis1_i*coeff_init(1:PDG_TFC_constant.Chebyshev_N);
        h1y_i = basis1_i*coeff_init(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N);
        h1z_i = basis1_i*coeff_init(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N);
        basis1_d1_i = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t0_guess)./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess).*2.-1,1);
        h1x_d1_i = basis1_d1_i*coeff_init(1:PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
        h1y_d1_i = basis1_d1_i*coeff_init(PDG_TFC_constant.Chebyshev_N+1:2*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
        h1z_d1_i = basis1_d1_i*coeff_init(2*PDG_TFC_constant.Chebyshev_N+1:3*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t1-PDG_TFC_parmeters.t0_guess);
        residual(1:3,i)=r1_i(:,i) - [h1x_i - switch1_f_i1*(h10x_i-PDG_TFC_constant.r0(1)) - ...
            switch1_f_i2*(h1fx_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+1,1)) - ...
                switch1_f_i3*(h10x_d1_i-PDG_TFC_constant.v0(1)) - ...
                    switch1_f_i4*(h1fx_d1_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+4,1));
                    h1y_i - switch1_f_i1*(h10y_i-PDG_TFC_constant.r0(2)) - ...
                        switch1_f_i2*(h1fy_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+2,1)) - ...
                            switch1_f_i3*(h10y_d1_i-PDG_TFC_constant.v0(2)) - ...
                                switch1_f_i4*(h1fy_d1_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+5,1));
                                h1z_i - switch1_f_i1*(h10z_i-PDG_TFC_constant.r0(3)) - ...
                        switch1_f_i2*(h1fz_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+3,1)) - ...
                            switch1_f_i3*(h10z_d1_i-PDG_TFC_constant.v0(3)) - ...
                                switch1_f_i4*(h1fz_d1_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+6,1))
                        ];
        residual(4:6,i)=v1_i(:,i) - [h1x_d1_i - switch1_f_d1_i1*(h10x_i - PDG_TFC_constant.r0(1)) -...
            switch1_f_d1_i2*(h1fx_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+1,1)) -...
                switch1_f_d1_i3*(h10x_d1_i - PDG_TFC_constant.v0(1)) -...
                    switch1_f_d1_i4*(h1fx_d1_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+4,1));
                    h1y_d1_i - switch1_f_d1_i1*(h10y_i - PDG_TFC_constant.r0(2)) -...
                        switch1_f_d1_i2*(h1fy_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+2,1)) -...
                            switch1_f_d1_i3*(h10y_d1_i - PDG_TFC_constant.v0(2)) -...
                                switch1_f_d1_i4*(h1fy_d1_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+5,1));
                                h1z_d1_i - switch1_f_d1_i1*(h10z_i - PDG_TFC_constant.r0(3)) -...
                        switch1_f_d1_i2*(h1fz_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+3,1)) -...
                            switch1_f_d1_i3*(h10z_d1_i - PDG_TFC_constant.v0(3)) -...
                                switch1_f_d1_i4*(h1fz_d1_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+6,1))];
        % residual(1:3,i)=r1_i(:,i) - [h1x_i - switch1_f_i1*(h10x_i-PDG_TFC_constant.r0(1)) - ...
        %     switch1_f_i2*(h1fx_i-r1_i(1,end)) - ...
        %         switch1_f_i3*(h10x_d1_i-PDG_TFC_constant.v0(1)) - ...
        %             switch1_f_i4*(h1fx_d1_i-v1_i(1,end));
        %             h1y_i - switch1_f_i1*(h10y_i-PDG_TFC_constant.r0(2)) - ...
        %                 switch1_f_i2*(h1fy_i-r1_i(2,end)) - ...
        %                     switch1_f_i3*(h10y_d1_i-PDG_TFC_constant.v0(2)) - ...
        %                         switch1_f_i4*(h1fy_d1_i-v1_i(2,end));
        %                         h1z_i - switch1_f_i1*(h10z_i-PDG_TFC_constant.r0(3)) - ...
        %                 switch1_f_i2*(h1fz_i-r1_i(3,end)) - ...
        %                     switch1_f_i3*(h10z_d1_i-PDG_TFC_constant.v0(3)) - ...
        %                         switch1_f_i4*(h1fz_d1_i-v1_i(3,end))];
        % residual(4:6,i)=v1_i(:,i) - [h1x_d1_i - switch1_f_d1_i1*(h10x_i - PDG_TFC_constant.r0(1)) -...
        %     switch1_f_d1_i2*(h1fx_i - r1_i(1,end)) -...
        %         switch1_f_d1_i3*(h10x_d1_i - PDG_TFC_constant.v0(1)) -...
        %             switch1_f_d1_i4*(h1fx_d1_i - v1_i(1,end));
        %             h1y_d1_i - switch1_f_d1_i1*(h10y_i - PDG_TFC_constant.r0(2)) -...
        %                 switch1_f_d1_i2*(h1fy_i - r1_i(2,end)) -...
        %                     switch1_f_d1_i3*(h10y_d1_i - PDG_TFC_constant.v0(2)) -...
        %                         switch1_f_d1_i4*(h1fy_d1_i - v1_i(2,end));
        %                         h1z_d1_i - switch1_f_d1_i1*(h10z_i - PDG_TFC_constant.r0(3)) -...
        %                 switch1_f_d1_i2*(h1fz_i - r1_i(3,end)) -...
        %                     switch1_f_d1_i3*(h10z_d1_i - PDG_TFC_constant.v0(3)) -...
        %                         switch1_f_d1_i4*(h1fz_d1_i - v1_i(3,end))];
    end
    % 计算第二段最优轨迹
    h20x_i = basis_0_i*coeff_init(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N);
    h20x_d1_i = basis_d0_i*coeff_init(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
    h2fx_i = basis_f_i*coeff_init(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N);
    h2fx_d1_i = basis_df_i*coeff_init(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
    h20y_i = basis_0_i*coeff_init(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N);
    h20y_d1_i = basis_d0_i*coeff_init(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
    h2fy_i = basis_f_i*coeff_init(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N);
    h2fy_d1_i = basis_df_i*coeff_init(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
    h20z_i = basis_0_i*coeff_init(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N);
    h20z_d1_i = basis_d0_i*coeff_init(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
    h2fz_i = basis_f_i*coeff_init(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N);
    h2fz_d1_i = basis_df_i*coeff_init(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
    r2_i = r_i(1:3,PDG_TFC_constant.discretize_points+1:2*PDG_TFC_constant.discretize_points);
    v2_i = v_i(1:3,PDG_TFC_constant.discretize_points+1:2*PDG_TFC_constant.discretize_points);
    for i = PDG_TFC_constant.discretize_points+1:1:2*PDG_TFC_constant.discretize_points
        switch2_f_i1 = tfc_constrain_table(t(i),'.','Initial_Value',2,PDG_TFC_parmeters);
        switch2_f_i2 = tfc_constrain_table(t(i),'.','Final_Value',2,PDG_TFC_parmeters);
        switch2_f_i3 = tfc_constrain_table(t(i),'.','Initial_Derivative',2,PDG_TFC_parmeters);
        switch2_f_i4 = tfc_constrain_table(t(i),'.','Final_Derivative',2,PDG_TFC_parmeters);
        switch2_f_d1_i1 = tfc_constrain_table(t(i),'d/dt','Initial_Value',2,PDG_TFC_parmeters);
        switch2_f_d1_i2 = tfc_constrain_table(t(i),'d/dt','Final_Value',2,PDG_TFC_parmeters);
        switch2_f_d1_i3 = tfc_constrain_table(t(i),'d/dt','Initial_Derivative',2,PDG_TFC_parmeters);
        switch2_f_d1_i4 = tfc_constrain_table(t(i),'d/dt','Final_Derivative',2,PDG_TFC_parmeters);
        basis2_i = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t1)./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1).*2.-1);
        h2x_i = basis2_i*coeff_init(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N);
        h2y_i = basis2_i*coeff_init(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N);
        h2z_i = basis2_i*coeff_init(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N);
        basis2_d1_i = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t1)./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1).*2.-1,1);
        h2x_d1_i = basis2_d1_i*coeff_init(3*PDG_TFC_constant.Chebyshev_N+1:4*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
        h2y_d1_i = basis2_d1_i*coeff_init(4*PDG_TFC_constant.Chebyshev_N+1:5*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
        h2z_d1_i = basis2_d1_i*coeff_init(5*PDG_TFC_constant.Chebyshev_N+1:6*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.t2-PDG_TFC_parmeters.t1);
        residual(1:3,i)=r2_i(:,i-PDG_TFC_constant.discretize_points) - [h2x_i - switch2_f_i1*(h20x_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+1,1)) - ...
            switch2_f_i2*(h2fx_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+7,1)) - ...
                switch2_f_i3*(h20x_d1_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+4,1)) - ...
                    switch2_f_i4*(h2fx_d1_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+10,1));
                    h2y_i - switch2_f_i1*(h20y_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+2,1)) - ...
                        switch2_f_i2*(h2fy_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+8,1)) - ...
                            switch2_f_i3*(h20y_d1_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+5,1)) - ...
                                switch2_f_i4*(h2fy_d1_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+11,1));
                    h2z_i - switch2_f_i1*(h20z_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+3,1)) - ...
                        switch2_f_i2*(h2fz_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+9,1)) - ...
                            switch2_f_i3*(h20z_d1_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+6,1)) - ...
                                switch2_f_i4*(h2fz_d1_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+12,1))
                            ];
        residual(4:6,i)=v2_i(:,i-PDG_TFC_constant.discretize_points) - [h2x_d1_i - switch2_f_d1_i1*(h20x_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+1,1)) -...
            switch2_f_d1_i2*(h2fx_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+7,1)) -...
                switch2_f_d1_i3*(h20x_d1_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+4,1)) -...
                    switch2_f_d1_i4*(h2fx_d1_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+10,1));
                    h2y_d1_i - switch2_f_d1_i1*(h20y_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+2,1)) -...
                        switch2_f_d1_i2*(h2fy_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+8,1)) -...
                            switch2_f_d1_i3*(h20y_d1_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+5,1)) -...
                                switch2_f_d1_i4*(h2fy_d1_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+11,1));
                                h2z_d1_i - switch2_f_d1_i1*(h20z_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+3,1)) -...
                        switch2_f_d1_i2*(h2fz_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+9,1)) -...
                            switch2_f_d1_i3*(h20z_d1_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+6,1)) -...
                                switch2_f_d1_i4*(h2fz_d1_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+12,1))];
        % residual(1:3,i)=r2_i(:,i-PDG_TFC_constant.discretize_points) - [h2x_i - switch2_f_i1*(h20x_i-r1_i(1,end)) - ...
        %     switch2_f_i2*(h2fx_i-r2_i(1,end)) - ...
        %         switch2_f_i3*(h20x_d1_i-v1_i(1,end)) - ...
        %             switch2_f_i4*(h2fx_d1_i-v2_i(1,end));
        %             h2y_i - switch2_f_i1*(h20y_i-r1_i(2,end)) - ...
        %                 switch2_f_i2*(h2fy_i-r2_i(2,end)) - ...
        %                     switch2_f_i3*(h20y_d1_i-v1_i(2,end)) - ...
        %                         switch2_f_i4*(h2fy_d1_i-v2_i(2,end));
        %                         h2z_i - switch2_f_i1*(h20z_i-r1_i(3,end)) - ...
        %                 switch2_f_i2*(h2fz_i-r2_i(3,end)) - ...
        %                     switch2_f_i3*(h20z_d1_i-v1_i(3,end)) - ...
        %                         switch2_f_i4*(h2fz_d1_i-v2_i(3,end))];
        % residual(4:6,i)=v2_i(:,i-PDG_TFC_constant.discretize_points) - [h2x_d1_i - switch2_f_d1_i1*(h20x_i - r1_i(1,end)) -...
        %     switch2_f_d1_i2*(h2fx_i - r2_i(1,end)) -...
        %         switch2_f_d1_i3*(h20x_d1_i - v1_i(1,end)) -...
        %             switch2_f_d1_i4*(h2fx_d1_i - v2_i(1,end));
        %             h2y_d1_i - switch2_f_d1_i1*(h20y_i - r1_i(2,end)) -...
        %                 switch2_f_d1_i2*(h2fy_i - r2_i(2,end)) -...
        %                     switch2_f_d1_i3*(h20y_d1_i - v1_i(2,end)) -...
        %                         switch2_f_d1_i4*(h2fy_d1_i - v2_i(2,end));
        %                         h2z_d1_i - switch2_f_d1_i1*(h20z_i - r1_i(3,end)) -...
        %                 switch2_f_d1_i2*(h2fz_i - r2_i(3,end)) -...
        %                     switch2_f_d1_i3*(h20z_d1_i - v1_i(3,end)) -...
        %                         switch2_f_d1_i4*(h2fz_d1_i - v2_i(3,end))];
    end
    % 计算第三段最优轨迹
    h30x_i = basis_0_i*coeff_init(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N);
    h30x_d1_i = basis_d0_i*coeff_init(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
    h3fx_i = basis_f_i*coeff_init(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N);
    h3fx_d1_i = basis_df_i*coeff_init(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
    h30y_i = basis_0_i*coeff_init(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N);
    h30y_d1_i = basis_d0_i*coeff_init(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
    h3fy_i = basis_f_i*coeff_init(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N);
    h3fy_d1_i = basis_df_i*coeff_init(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
    h30z_i = basis_0_i*coeff_init(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N);
    h30z_d1_i = basis_d0_i*coeff_init(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
    h3fz_i = basis_f_i*coeff_init(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N);
    h3fz_d1_i = basis_df_i*coeff_init(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
    r3_i = r_i(1:3,2*PDG_TFC_constant.discretize_points+1:3*PDG_TFC_constant.discretize_points);
    v3_i = v_i(1:3,2*PDG_TFC_constant.discretize_points+1:3*PDG_TFC_constant.discretize_points);
    for i = 2*PDG_TFC_constant.discretize_points+1:1:3*PDG_TFC_constant.discretize_points
        switch3_f_i1 = tfc_constrain_table(t(i),'.','Initial_Value',3,PDG_TFC_parmeters);
        switch3_f_i2 = tfc_constrain_table(t(i),'.','Final_Value',3,PDG_TFC_parmeters);
        switch3_f_i3 = tfc_constrain_table(t(i),'.','Initial_Derivative',3,PDG_TFC_parmeters);
        switch3_f_i4 = tfc_constrain_table(t(i),'.','Final_Derivative',3,PDG_TFC_parmeters);
        switch3_f_d1_i1 = tfc_constrain_table(t(i),'d/dt','Initial_Value',3,PDG_TFC_parmeters);
        switch3_f_d1_i2 = tfc_constrain_table(t(i),'d/dt','Final_Value',3,PDG_TFC_parmeters);
        switch3_f_d1_i3 = tfc_constrain_table(t(i),'d/dt','Initial_Derivative',3,PDG_TFC_parmeters);
        switch3_f_d1_i4 = tfc_constrain_table(t(i),'d/dt','Final_Derivative',3,PDG_TFC_parmeters);
        basis3_i = chebyshev_basis(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t2)./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2).*2.-1);
        h3x_i = basis3_i*coeff_init(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N);
        h3y_i = basis3_i*coeff_init(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N);
        h3z_i = basis3_i*coeff_init(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N);
        basis3_d1_i = chebyshev_derivative(PDG_TFC_constant.Chebyshev_N,(t(i)-PDG_TFC_parmeters.t2)./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2).*2.-1,1);
        h3x_d1_i = basis3_d1_i*coeff_init(6*PDG_TFC_constant.Chebyshev_N+1:7*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
        h3y_d1_i = basis3_d1_i*coeff_init(7*PDG_TFC_constant.Chebyshev_N+1:8*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
        h3z_d1_i = basis3_d1_i*coeff_init(8*PDG_TFC_constant.Chebyshev_N+1:9*PDG_TFC_constant.Chebyshev_N)*2./(PDG_TFC_parmeters.tf_guess-PDG_TFC_parmeters.t2);
        residual(1:3,i)=r3_i(:,i-2*PDG_TFC_constant.discretize_points) - [h3x_i - switch3_f_i1*(h30x_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+7,1)) - ...
            switch3_f_i2*(h3fx_i-PDG_TFC_constant.rf(1)) - ...
               switch3_f_i3*(h30x_d1_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+10,1)) - ...
                   switch3_f_i4*(h3fx_d1_i-PDG_TFC_constant.vf(1));
                    h3y_i - switch3_f_i1*(h30y_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+8,1)) - ...
                        switch3_f_i2*(h3fy_i-PDG_TFC_constant.rf(2)) - ...
                            switch3_f_i3*(h30y_d1_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+11,1)) - ...
                               switch3_f_i4*(h3fy_d1_i-PDG_TFC_constant.vf(2));
                                h3z_i - switch3_f_i1*(h30z_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+9,1)) - ...
                        switch3_f_i2*(h3fz_i-PDG_TFC_constant.rf(3)) - ...
                            switch3_f_i3*(h30z_d1_i-coeff_init(9*PDG_TFC_constant.Chebyshev_N+12,1)) - ...
                                switch3_f_i4*(h3fz_d1_i-PDG_TFC_constant.vf(3))
                                ];
        residual(4:6,i)=v3_i(:,i-2*PDG_TFC_constant.discretize_points) - [h3x_d1_i - switch3_f_d1_i1*(h30x_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+7,1)) -...
            switch3_f_d1_i2*(h3fx_i - PDG_TFC_constant.rf(1)) -...
                switch3_f_d1_i3*(h30x_d1_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+10,1)) -...
                    switch3_f_d1_i4*(h3fx_d1_i - PDG_TFC_constant.vf(1));
                    h3y_d1_i - switch3_f_d1_i1*(h30y_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+8,1)) -...
                        switch3_f_d1_i2*(h3fy_i - PDG_TFC_constant.rf(2)) -...
                            switch3_f_d1_i3*(h30y_d1_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+11,1)) -...
                                switch3_f_d1_i4*(h3fy_d1_i - PDG_TFC_constant.vf(2));
                                h3z_d1_i - switch3_f_d1_i1*(h30z_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+9,1)) -...
                        switch3_f_d1_i2*(h3fz_i - PDG_TFC_constant.rf(3)) -...
                            switch3_f_d1_i3*(h30z_d1_i - coeff_init(9*PDG_TFC_constant.Chebyshev_N+12,1)) -...
                                switch3_f_d1_i4*(h3fz_d1_i - PDG_TFC_constant.vf(3))];
        % residual(1:3,i)=r3_i(:,i-2*PDG_TFC_constant.discretize_points) - [h3x_i - switch3_f_i1*(h30x_i-r2_i(1,end)) - ...
        %     switch3_f_i2*(h3fx_i-PDG_TFC_constant.rf(1)) - ...
        %         switch3_f_i3*(h30x_d1_i-v2_i(1,end)) - ...
        %             switch3_f_i4*(h3fx_d1_i-PDG_TFC_constant.vf(1));
        %             h3y_i - switch3_f_i1*(h30y_i-r2_i(2,end)) - ...
        %                 switch3_f_i2*(h3fy_i-PDG_TFC_constant.rf(2)) - ...
        %                     switch3_f_i3*(h30y_d1_i-v2_i(2,end)) - ...
        %                         switch3_f_i4*(h3fy_d1_i-PDG_TFC_constant.vf(2));
        %                         h3z_i - switch3_f_i1*(h30z_i-r2_i(3,end)) - ...
        %                 switch3_f_i2*(h3fz_i-PDG_TFC_constant.rf(3)) - ...
        %                     switch3_f_i3*(h30z_d1_i-v2_i(3,end)) - ...
        %                         switch3_f_i4*(h3fz_d1_i-PDG_TFC_constant.vf(3))];
        % residual(4:6,i)=v3_i(:,i-2*PDG_TFC_constant.discretize_points) - [h3x_d1_i - switch3_f_d1_i1*(h30x_i - r2_i(1,end)) -...
        %     switch3_f_d1_i2*(h3fx_i - PDG_TFC_constant.rf(1)) -...
        %         switch3_f_d1_i3*(h30x_d1_i - v2_i(1,end)) -...
        %             switch3_f_d1_i4*(h3fx_d1_i - PDG_TFC_constant.vf(1));
        %             h3y_d1_i - switch3_f_d1_i1*(h30y_i - r2_i(2,end)) -...
        %                 switch3_f_d1_i2*(h3fy_i - PDG_TFC_constant.rf(2)) -...
        %                     switch3_f_d1_i3*(h30y_d1_i - v2_i(2,end)) -...
        %                         switch3_f_d1_i4*(h3fy_d1_i - PDG_TFC_constant.vf(2));
        %                         h3z_d1_i - switch3_f_d1_i1*(h30z_i - r2_i(3,end)) -...
        %                 switch3_f_d1_i2*(h3fz_i - PDG_TFC_constant.rf(3)) -...
        %                     switch3_f_d1_i3*(h30z_d1_i - v2_i(3,end)) -...
        %                         switch3_f_d1_i4*(h3fz_d1_i - PDG_TFC_constant.vf(3))];
    end
    % 协态变量残差
    %for i=1:1:3*PDG_TFC_constant.discretize_points
        residual(1,3*PDG_TFC_constant.discretize_points+1) = lamda_v0(1,1) -  ...
        [coeff_init(9*PDG_TFC_constant.Chebyshev_N+13,1),coeff_init(9*PDG_TFC_constant.Chebyshev_N+14,1)]*[1;t(1)];
        residual(2,3*PDG_TFC_constant.discretize_points+1) = lamda_v0(2,1) -  ...
        [coeff_init(9*PDG_TFC_constant.Chebyshev_N+15,1),coeff_init(9*PDG_TFC_constant.Chebyshev_N+16,1)]*[1;t(1)];
        residual(3,3*PDG_TFC_constant.discretize_points+1) = lamda_v0(3,1) -  ...
        [coeff_init(9*PDG_TFC_constant.Chebyshev_N+17,1),coeff_init(9*PDG_TFC_constant.Chebyshev_N+18,1)]*[1;t(1)];
        residual(4,3*PDG_TFC_constant.discretize_points+1) = lamda_vf(1,1) -  ...
        [coeff_init(9*PDG_TFC_constant.Chebyshev_N+13,1),coeff_init(9*PDG_TFC_constant.Chebyshev_N+14,1)]*[1;t(end)];
        residual(5,3*PDG_TFC_constant.discretize_points+1) = lamda_vf(2,1) -  ...
        [coeff_init(9*PDG_TFC_constant.Chebyshev_N+15,1),coeff_init(9*PDG_TFC_constant.Chebyshev_N+16,1)]*[1;t(end)];
        residual(6,3*PDG_TFC_constant.discretize_points+1) = lamda_vf(3,1) -  ...
        [coeff_init(9*PDG_TFC_constant.Chebyshev_N+17,1),coeff_init(9*PDG_TFC_constant.Chebyshev_N+18,1)]*[1;t(end)];

    %end
    %disp(residual)
end
    